#ifndef PIVOTS_H_
#define PIVOTS_H_

#include "pid.h"
#include "rclcpp/rclcpp.hpp"
#include <memory> 
#include <rclcpp_lifecycle/lifecycle_node.hpp> 
#include "chassis_controller/hardware_config.h"

// 是否开启ROS环境下的log输出/std::cout
#define ROS_ENV_OUT
#define DEBUG_OUT_ENABLE

#ifdef DEBUG_OUT_ENABLE
#ifdef ROS_ENV_OUT
#define DEBUG_OUT(...) RCLCPP_INFO(rclcpp::get_logger("chassis_controller"), __VA_ARGS__)
#define DEBUG_STREAM_OUT(info) \
    RCLCPP_INFO_STREAM(rclcpp::get_logger("chassis_controller"), info)
#define DEBUG_OUT_THROTTLE(clock,s,info) \
    RCLCPP_INFO_STREAM_THROTTLE(rclcpp::get_logger("chassis_controller"), clock, s, info) 
#define DEBUG_WARN_OUT(info) \
    RCLCPP_WARN_STREAM(rclcpp::get_logger("chassis_controller"), info)
#define DEBUG_ERROR_OUT(info) \
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("chassis_controller"), info)
#define DEBUG_FATAL_OUT(info) \
    RCLCPP_FATAL_STREAM(rclcpp::get_logger("chassis_controller"), info)

#define DEBUG_WARN_ON_TRANSITION(condition, message) \
    do { \
        static bool last_##condition = true; \
        static bool triggered_##condition = false; \
        if (!(condition)) { \
            if (last_##condition && !triggered_##condition) { \
                DEBUG_WARN_OUT(message); \
                triggered_##condition = true; \
            } \
            last_##condition = false; \
        } else { \
            last_##condition = true; \
            triggered_##condition = false; \
        } \
    } while (0)
#else
#include <iostream>
#define DEBUG_OUT(info) std::cout << info << std::endl
#define DEBUG_WARN_OUT(info) std::cout << info << std::endl
#define DEBUG_ERROR_OUT(info) std::cout << info << std::endl
#define DEBUG_FATAL_OUT(info) std::cout << info << std::endl
#endif
#else
#define DEBUG_OUT(info)
#define DEBUG_WARN_OUT(info)
#define DEBUG_ERROR_OUT(info)
#define DEBUG_FATAL_OUT(info)
#endif

constexpr double PI = 3.141592653589793238462643383279502884;
constexpr double PI_2 = (PI / 2.0);
constexpr double _2_PI = (PI * 2.0);
constexpr double PI_4 = (PI / 4.0);
#define TO_DEGREE(x) (x / PI * 180.0)
#define TO_RADIAN(x) (x / 180.0 * PI)

enum chassis_type
{
    MULTI_PIVOTS = 0,
    DIFF = 1,
    TRICYLE = 2
};
// 驱动轴的实际值
struct Sjoint_value
{
    std::string name; // 电机名（关节名）
    double position;  // 电机实际位置[m]
    double velocity;  // 电机实际速度[m/s]
    double current;  // 电机实际电流[A]
    Sjoint_value()
    {
        name = "";
        position = 0.0;
        velocity = 0.0;
    }
};
//驱动单元的实际值
struct Spivot_value
{
    double steer_position; // 驱动单元角度[rad]
    double velocity;       // 驱动单元速度[m/s]
    double position;       // 驱动单元位置[m]
    double current;        // 驱动单元总电流[A]
    double external_angle; // 外部角度编码器值[rad]
    bool inverted;         // 计算已取反
    bool steering;         // 正在转舵
    bool steer_follow_err; // 转舵跟随超差
    bool stop_req;         // 停止请求
    std::vector<Sjoint_value> joint_value;
    Spivot_value()
    {
        steer_position = 0.0;
        velocity = 0.0;
        position = 0.0;
        external_angle = 0.0;
        inverted = false;
        steering = false;
        steer_follow_err = false;
        stop_req = false;
        joint_value.clear();
    }
};

// 驱动轴的目标值
struct Sjoint_setpoint
{
    double position_setpoint; // 电机目标位置[m]
    double velocity_setpoint; // 电机目标速度[m/s]
    Sjoint_setpoint()
    {
        position_setpoint = 0.0;
        velocity_setpoint = 0.0;
    }
};
// 驱动单元的目标值
struct Spivot_setpoint
{
    double steer_setpoint;    // 驱动单元目标角度[rad]
    double velocity_setpoint; // 驱动单元目标速度[m/s]
    bool invert;              // 强制向左转
    bool forbidden_invert;    // 禁止向右转
    bool suspend_invert;      // 暂停反向，保持现状
    std::vector<Sjoint_setpoint> joint_setpoint;
    Spivot_setpoint()
    {
        steer_setpoint = 0.0;
        velocity_setpoint = 0.0;
        invert = false;
        forbidden_invert = false;
        suspend_invert = false;
        joint_setpoint.clear();
    }
};

// 驱动轴的参数
enum ServoModel : int
{
    KINCO = 0,
    ZAPI = 1,
    CURTIS = 2,
    AMC = 3
};

enum ServoDriverMode : int
{
    STEER = 1,
    TRACTION = 0
};

//各驱动电机的参数
struct Sjoint_param
{
    std::string name;      // 电机名（关节名）
    int modle;             // 电机类型
    int ID;                //
    int drive_mode;        // 电机是用于转向还是行走
    int resolution;        // 分辨率
    double wheel_diameter; // 驱动轮直径
    GenericHandle handle;
    Sjoint_param()
    {
        name = "";
        modle = 0;
        ID = 0;
        drive_mode = 0;
        resolution = 0;
        wheel_diameter = 0.0;
    }
};

enum pivot : int
{
    DIFFWHEEL = 0,
    STEERWHEEL = 1,
    OTHER = 2
};
//驱动单元参数
struct Spivot_param
{
    std::string name;                      // 驱动单元名
    int pivot_type;                        // 驱动单元类型
    double driving_wheel_diameter;         // 驱动轮直径
    double radius_ringmount;               // 差速单元回转半径
    double steering_ratio;                 // 转向电机减速比
    double traction_ratio;                 // 牵引电机减速比
    int slewing_gear;                      // 回转支撑大齿轮齿数
    int drive_gear;                        // 转向减速机齿数
    int feedback_gear;                     // 反馈编码器齿轮齿数
    double steer_offset;                   // 舵角零偏值[rad]
    double velocity_factor;                // 速度系数
    double odometry_factor;                // 里程计系数
    bool use_external_encoder;             // 是否使用外部编码器
    bool steer_feed_invert;                // 角度反馈反向
    int external_encoder_resolution;       // 外部编码器分辨率（4倍频后的值）              
    std::string external_encoder_name;     // 驱动单元名
    GenericHandle encoder_handle;
    double offset_x;                       // 安装位置X
    double offset_y;                       // 安装位置Y
    double max_angle;                      // 最大转角正向（左）
    double min_angle;                      // 最大转角负值（右）
    std::vector<Sjoint_param> joint_param; // 组成驱动单元的两个电机的参数
    Spivot_param()
    {
        name = "";
        pivot_type = 0;
        driving_wheel_diameter = 0.0;
        radius_ringmount = 0.0;
        steering_ratio = 0.0;
        traction_ratio = 0.0;
        slewing_gear = 0;
        drive_gear = 0;
        feedback_gear = 0;
        steer_offset = 0.0;
        velocity_factor = 0.0;
        use_external_encoder = false;
        steer_feed_invert = false;
        external_encoder_resolution = 0;
        offset_x = 0.0;
        offset_y = 0.0;
        max_angle = 0.0;
        min_angle = 0.0;
        joint_param.clear();
    }
};
struct Sxya
{
    double x;
    double y;
    double a;
    Sxya()
    {
        x = 0.0;
        y = 0.0;
        a = 0.f;
    }
};


#endif